/**
 * Author: luoqi
 * Created Date: 2025-11-15 01:34:17
 * Last Modified: 2025-11-20 00:02:36
 * Modified By: luoqi at <**@****>
 * Copyright (c) 2025 <*****>
 * Description:
 */

#ifndef _MC_H_
#define _MC_H_

#include <stdint.h>
#include "ctrl/lpf_1st.h"
#include "ctrl/pid.h"

typedef enum {
    MC_MOD_IDLE = 0,
    MC_MOD_VEL,
    MC_MOD_POS,
} McMod;

typedef struct {
    McMod mod; // motor control mode

    float pos; // current position
    
    float vel; // current velocity
    
    float pref; // reference position
    float vref; // reference velocity

    float ep_max; // max position error

    PidObj vpid; // velocity pid controller
    PidObj ppid; // position pid controller

    Lpf1stObj vlpf; // velocity low pass filter
    Lpf1stObj ovlpf; // output low pass filter
} McObj;

int mc_init(McObj *mc);

int mc_vpid_set(McObj *mc, float kp, float ki, float kd, float omax);

int mc_ppid_set(McObj *mc, float kp, float ki, float kd, float omax);

int mc_vlpf_set(McObj *mc, float fc, float fs);

int mc_ovlpf_set(McObj *mc, float fc, float fs);

int mc_ep_max_set(McObj *mc, float ep_max);

int mc_mod_set(McObj *mc, McMod mod);

int mc_pos_update(McObj *mc, float pos);

int mc_vel_update(McObj *mc, float vel);

float mc_vloop(McObj *mc, float vref, float dt);

float mc_ploop(McObj *mc, float pref, float dt);

#endif
